#pragma once
#include "Body.h"
using namespace Eigen;
class JointBase
{
	typedef Matrix<double, 3, 4> MatR3C4;
protected:
	Body* Bi;                              //pointer to outer body.
	Body* Bj;                              //pointer to inner body.
	double* yi;                            //generalized coordinates.
	double* dyi;                           //generalized velocity.
	Vector3d rhoi;                         //rhop vector
	Vector3d rhoj;                         //rhoq vector
	Matrix3d CiP;                          //transform from P to h.   Constant
	Matrix3d CjQ;                          //transform from Q to h0.  Constant
	Matrix3d Dih;                          //transform from h0 to h.
protected:
	Matrix3d& calCiP();
	Matrix3d& calCjQ();
	virtual bool calAh0(OUT Matrix3d& A);
	virtual Vector3d Hi()const;
	virtual Vector3d Wri()const;
	virtual Vector3d Vri()const;
private:
	/*need derived class to complete it.*/
	virtual MatR3CX HhT()const = 0;
	virtual MatR3CX HoT()const = 0;
	virtual Vector3d Yita()const = 0;                                             
	virtual Matrix3d& calDih() = 0;
public:
	enum
	{
		REVOLUTIONAL = 0x00,
		UNIVERSAL = 0x01,
		SPHERICAL=0x02,
		PRISMATIC=0x03,
		CYLINDRICAL=0x04,
		VIRTUAL=0x05,

		REVOLUTIONALDRIVE = 0x10,
		UNIVERSALDRIVE = 0x11,
		SPHERICALDRIVE = 0x12,
		PRISMATICDRIVE = 0x13,
		CYLINDRICALDRIVE = 0x14,
		VIRTUALDRIVE = 0x15,
	};
	friend class MBSystem;
	friend class MBFileParser;
	JointBase(IN Body* Bi_ptr,IN Body* Bj_ptr,IN const Vector3d& rho_i,IN const Vector3d& rho_j);
	virtual ~JointBase();
	bool operator<(IN const JointBase& other)const;
	int GetBiId()const;
	int GetBjId()const;
	/*need derived class to complete it.*/
	inline virtual unsigned int DOF()const = 0;
	inline virtual unsigned int Type()const = 0;
	/*some common functions*/
	virtual bool calytoq(IN double t,IN double* y);                             //calculate from y to q
	virtual bool caldytodq(IN double t, IN double* dy);                         //calculate from dy to dq
	virtual bool setCiP(IN const Matrix3d& c);                 //CiP CjQ is transformation matrix from
	virtual bool setCjQ(IN const Matrix3d& c);                 //element cords to joint cords
	virtual Vector3d YitaPlusZeta();                           //return Yita+Zeta
	virtual bool CalHoT(MatR3CX& hot);
	virtual bool CalHhT(MatR3CX& hht);
	virtual bool Write(OUT Json::Value& joint);
	
};

/*the rotation axis is along x-axis by default.*/
class Revolute :public JointBase
{
	typedef Matrix<double, 3, -1> MatR3CX;
private:
	virtual MatR3CX HhT()const;
	virtual MatR3CX HoT()const;
	virtual Vector3d Yita()const;
	virtual Matrix3d& calDih();
public:
	Revolute(IN Body* Bi_ptr,IN Body* Bj_ptr,IN Vector3d& rho_i,IN Vector3d& rho_j);
	~Revolute() = default;
	inline unsigned int DOF()const;
	virtual inline unsigned int Type()const;
};

class Universe :public JointBase
{
	typedef Matrix<double, 3, -1> MatR3CX;
private:
	virtual MatR3CX HhT()const;
	virtual MatR3CX HoT()const;
	virtual Vector3d Yita()const;
	virtual Matrix3d& calDih();
public:
	Universe(IN Body* Bi_ptr,IN Body* Bj_ptr,IN Vector3d& rho_i,IN Vector3d& rho_j);
	~Universe() = default;
	inline unsigned int DOF()const;
	virtual inline unsigned int Type()const;
};

class Virtual :public JointBase
{
	typedef Matrix<double, 3, -1> MatR3CX;
private:
	virtual MatR3CX HhT()const;
	virtual MatR3CX HoT()const;
	virtual Vector3d Yita()const;
	virtual Matrix3d& calDih();
public:
	Virtual(IN Body* Bi_ptr,IN Body* Bj_ptr,IN Vector3d& rho_i,IN Vector3d& rho_j);
	~Virtual() = default;
	inline unsigned int DOF()const;
	virtual inline unsigned int Type()const;
};

class Sphere :public JointBase
{
	typedef Matrix<double, 3, -1> MatR3CX;
private:
	virtual MatR3CX HhT()const;
	virtual MatR3CX HoT()const;
	virtual Vector3d Yita()const;
	virtual Matrix3d& calDih();
public:
	Sphere(IN Body* Bi_ptr,IN Body* Bj_ptr,IN Vector3d& rho_i,IN Vector3d& rho_j);
	~Sphere() = default;
	inline unsigned int DOF()const;
	virtual inline unsigned int Type()const;
};

class Prism :public JointBase
{
	typedef Matrix<double, 3, -1> MatR3CX;
private:
	virtual MatR3CX HhT()const;
	virtual MatR3CX HoT()const;
	virtual Vector3d Yita()const;
	virtual Matrix3d& calDih();
public:
	Prism(IN Body* Bi_ptr,IN Body* Bj_ptr,IN Vector3d& rho_i,IN Vector3d& rho_j);
	~Prism() = default;
	inline unsigned int DOF()const;
	virtual inline unsigned int Type()const;
};

class Cylinder :public JointBase
{
	typedef Matrix<double, 3, -1> MatR3CX;
private:
	virtual MatR3CX HhT()const;
	virtual MatR3CX HoT()const;
	virtual Vector3d Yita()const;
	virtual Matrix3d& calDih();
public:
	Cylinder(IN Body* Bi_ptr,IN Body* Bj_ptr,IN Vector3d& rho_i,IN Vector3d& rho_j);
	~Cylinder() = default;
	inline unsigned int DOF()const;
	virtual inline unsigned int Type()const;
};

